/**
 * @file drv_motor.c
 * @brief 电机封装驱动
 * @author wangh (wanghuan3037@fiberhome.com)
 * @version 1.0
 * @date 2020-08-10
 * @copyright Copyright (c) 2020  烽火通信
 ***************************************************************
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version  <th>Author  <th>Description
 * <tr><td>2020-08-10 <td>1.0      <td>wangh   <td>内容
 * </table>
 */ 
#include "drv_motor.h"

#if BOARD_MOTOR_EN
#include "easyflash.h"

#undef LOG_LEVEL_
#define LOG_LEVEL_ 	LOG_LVL_DEBUG	/* TODO:LOG_LVL_WARN */
#include "bsp_log.h"

static MOTORS_CONTROL_S *g_motors = NULL;	/* 电机数组 */
static uint8_t g_motor_num = 10;			/* 电机组数量 */

static void motor_stop(MOTORS_CONTROL_S *mtr, bool isTimeout, bool isISR);
static void motor_set_start_state(MOTORS_CONTROL_S *mtr, MOTOR_START_STATE m_start);
static uint8_t get_motor_startRun_count(void);


/**
 * @brief 电机自动停止超时计时器
 * @param  xTimer	传递电机句柄
 */
static void mtr_autoStop_timeout_ind(TimerHandle_t xTimer)
{
	MOTORS_CONTROL_S *mtr = (MOTORS_CONTROL_S * )pvTimerGetTimerID(xTimer);
	motor_stop(mtr, true, false);
	
	log_d("motor%d location stop when timeout\r\n", mtr->index);
}

/**
 * @brief 电机启动计时器
 */
static void mtr_start_timeout_ind(TimerHandle_t xTimer)
{
	MOTORS_CONTROL_S *mtr = (MOTORS_CONTROL_S * )pvTimerGetTimerID(xTimer);
	motor_set_start_state(mtr, M_START_STOP);
	
	log_d("motor%d start over timeout\r\n", mtr->index);
}

#if USE_BSP_BUTTON
/**
 * @brief 电机实例初始化
 * @param  mtr        		电机实例
 * @param  btn_handle      	电机传感事件处理
 */
static void motor_init(MOTORS_CONTROL_S *mtr, button_handler_t btn_handle)
{
	GpioInit( &mtr->Motor_ph1, mtr->Motor_ph1_pin, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
	GpioInit( &mtr->Motor_ph2, mtr->Motor_ph2_pin, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
	GpioInit( &mtr->Motor_set, mtr->Motor_set_pin, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	
	button_init(&mtr->M_sensor_int, mtr->M_sensor_int_pin, PIN_NO_PULL, BUTTON_ACTIVE_LOW, BUTTON_DETECT_DELAY, btn_handle);
	

	/* 电机状态读写锁 */
	mtr->state_lock = xSemaphoreCreateBinary();	/* 初始时信号值为0 */
	if(mtr->state_lock == NULL)
	{
		log_e("motor%d state_lock create error\r\n", mtr->index);
		return;
	}
	xSemaphoreGive(mtr->state_lock);
	
	/* 电机自动停止定时器 */
	mtr->mtr_autoStop_timer = xTimerCreate("mtrstop", MAX_MOVE_TIME * portTICK_RATE_MS, pdFALSE, mtr, mtr_autoStop_timeout_ind);
	if(mtr->mtr_autoStop_timer == NULL)
	{
		log_e("motor%d mtr_autoStop_timer create error\r\n", mtr->index);
		return;
	}

	/* 电机启动定时器 */
	mtr->mtr_start_timer = xTimerCreate("mtrstart", MOTOR_START_DELAY * portTICK_RATE_MS, pdFALSE, mtr, mtr_start_timeout_ind);
	if(mtr->mtr_start_timer == NULL)
	{
		log_e("motor%d mtr_start_timer create error\r\n", mtr->index);
		return;
	}
}
#else
static void motor_init(MOTORS_CONTROL_S *mtr, GpioIrqHandler *irqHandler)
{
	GpioInit( &mtr->Motor_ph1, mtr->Motor_ph1_pin, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
	GpioInit( &mtr->Motor_ph2, mtr->Motor_ph2_pin, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
	GpioInit( &mtr->Motor_set, mtr->Motor_set_pin, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	GpioInit( &mtr->M_sensor_int, mtr->M_sensor_int_pin, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 2 );
	
	GpioSetInterrupt( &mtr->M_sensor_int, IRQ_RISING_FALLING_EDGE, IRQ_HIGH_PRIORITY, irqHandler );
	
	mtr->stopSemaphore = xSemaphoreCreateBinary();
	if(mtr->stopSemaphore == NULL)
	{
		log_e("motor%d stopSemaphore create error\r\n", mtr->index);
		return;
	}
	
	mtr->mtr_timer = xTimerCreate("mtr", MAX_MOVE_TIME * portTICK_RATE_MS, pdFALSE, mtr, mtr_timer_timeout_ind);
	if(mtr->mtr_timer == NULL)
	{
		log_e("motor%d mtr_timer create error\r\n", mtr->index);
		return;
	}
}
#endif

#if 0	/* 该接口已遗弃仅作以后参考 */
/**
 * @brief 电机操作
 * @param  mtr_list    待操作电机链表
 * @param  smove       动作
 */
static void motor_order(MOTORS_CONTROL_S *mtr_list, MOTOR_ACTION smove)
{
	MOTORS_CONTROL_S *elt, *tmp;

	log_d("motor_forward1 contorl: ");
	DL_FOREACH(mtr_list, elt)
	{
		log_raw("%d ", elt->index);
	}
	log_raw("\r\n");
	
	
	MOTORS_CONTROL_S *mtr_list_g = NULL;	/* 暂存分组处理链表 */
	DL_FOREACH_SAFE(mtr_list, elt, tmp)
	{
		if(elt->group_value > 0)
		{
			DL_DELETE(mtr_list, elt);
			DL_APPEND(mtr_list_g, elt);
		}
	}
	
	log_d("mtr_list: ");
	DL_FOREACH(mtr_list, elt)
	{
		log_raw("%d(%d) ", elt->index, elt->group_value);
	}
	log_raw("\r\n");
	log_d("mtr_list_g: ");
	DL_FOREACH(mtr_list_g, elt)
	{
		log_raw("%d(%d) ", elt->index, elt->group_value);
	}
	log_raw("\r\n");

	
	uint8_t move_group = 0;
	DL_FOREACH(mtr_list_g, elt)
	{
//		log_d("move_group=%d index=%d group=%d\r\n", move_group, elt->index, elt->group_value);
		if(elt->group_value == move_group)
		{
//			log_d("continue\r\n");
			continue;
		}
		
		log_d("motor move: ");
		for(uint8_t i=0; i<g_motor_num; i++)
		{
			if(g_motors[i].group_value == elt->group_value)
			{
				move_group = elt->group_value;
				log_raw("%d ", g_motors[i].index);
				// motor_forward_one(&g_motors[i]);
			}
		}
		log_raw("delay\r\n");
		hal_delay_ms(50);
	}
	
	uint8_t move_count = 0;
	log_d("motor move: ");
	DL_FOREACH(mtr_list, elt)
	{
		log_raw("%d ", elt->index);
		move_count++;
		
		if(move_count == 3)
		{
			move_count = 0;
			hal_delay_ms(50);
			log_raw("delay  ");
		}
	}
	log_raw("\r\n");
}
#endif

/**
 * @brief 设置电机状态
 * @warning	不适用与中断调用
 * @param  mtr         电机句柄
 * @param  m_state     电机动作
 * @param  m_start     电机启动状态
 * @param  m_reset     电机复位状态
 */
static void motor_set_state(MOTORS_CONTROL_S *mtr, MOTOR_ACTION m_state, MOTOR_START_STATE m_start, bool m_reset)
{
	xSemaphoreTake(mtr->state_lock, portMAX_DELAY);
	mtr->move_state = m_state;
	mtr->move_start = m_start;
	mtr->move_reset = m_reset;
	xSemaphoreGive(mtr->state_lock);
}

/**
 * @brief 设置电机启动状态
 */
static void motor_set_start_state(MOTORS_CONTROL_S *mtr, MOTOR_START_STATE m_start)
{
	xSemaphoreTake(mtr->state_lock, portMAX_DELAY);
	mtr->move_start = m_start;
	xSemaphoreGive(mtr->state_lock);
}

/**
 * @brief 获取当前电机动作
 */
static MOTOR_ACTION motor_get_move_state(MOTORS_CONTROL_S *mtr)
{
	xSemaphoreTake(mtr->state_lock, portMAX_DELAY);
	MOTOR_ACTION state = mtr->move_state;
	xSemaphoreGive(mtr->state_lock);
	return state;
}

/**
 * @brief 获取电机启动状态
 */
static MOTOR_START_STATE motor_get_start_state(MOTORS_CONTROL_S *mtr)
{
	xSemaphoreTake(mtr->state_lock, portMAX_DELAY);
	MOTOR_START_STATE state = mtr->move_start;
	xSemaphoreGive(mtr->state_lock);
	return state;
}

/**
 * @brief 设置电机复位状态
 */
bool motor_get_reset_state(MOTORS_CONTROL_S *mtr)
{
	xSemaphoreTake(mtr->state_lock, portMAX_DELAY);
	bool state = mtr->move_reset;
	xSemaphoreGive(mtr->state_lock);
	return state;
}

/**
 * @brief 单个电机停止动作 \n
 */
static void motor_stop_action(MOTORS_CONTROL_S *mtr)
{
	xTimerStop(mtr->mtr_autoStop_timer, 0);
	xTimerStop(mtr->mtr_start_timer, 0);
	GpioWrite( &mtr->Motor_ph1, 0 );
	GpioWrite( &mtr->Motor_ph2, 0 );
}

/**
 * @brief 单个电机停止处理 \n
 * 要求中断安全
 * @param  mtr         电机句柄
 * @param  isTimeout   是否为动作超时引起的停止
 * @param  isISR       是否为传感中断引起的停止
 */
static void motor_stop_handle(MOTORS_CONTROL_S *mtr, bool isTimeout, bool isISR)
{
	if(mtr->move_state == M_STOP)		/* 已停止退出处理 */
		return;

	/* 动作超时仍未复位成功则下移 */
	if((isTimeout) && (mtr->move_reset))
	{
		if(mtr->move_state == M_FORWARD)
			motor_set_state(mtr, M_BACK, M_START_READY, false);	/* 设置为true没有中点时会一直尝试复位 */
		else if(mtr->move_state == M_BACK)
			motor_set_state(mtr, M_FORWARD, M_START_READY, false);	
		
		log_d("motor%d stop time\r\n", mtr->index);
		return;
	}

	/* 	处理指令和传感中断停止 */
	if(isISR)	/* 复位中断 */
	{
		mtr->move_state = M_STOP;
		mtr->move_start = M_START_STOP;
		mtr->move_reset = false;
		log_d("motor%d stop isr\r\n", mtr->index);
	}
	else		/* 常规指令停止 */
	{
		motor_set_state(mtr, M_STOP, M_START_STOP, false);
		log_d("motor%d stop\r\n", mtr->index);
	}
	
	if((GpioRead(&mtr->M_sensor_int) == 0) &&(mtr->location_value != M_CENTER))
	{
		mtr->location_value = M_CENTER;
		ef_set_env_blob(mtr->location_key, &mtr->location_value, 1);
		log_d("motor%d location stop at M_CENTER\r\n", mtr->index);
	}
}

/**
 * @brief 电机停止 \n
 * 电机为分组时执行整个分组
 * @param  mtr         电机
 * @param  isTimeout   超时调用
 * @param  isISR       中断调用
 */
static void motor_stop(MOTORS_CONTROL_S *mtr, bool isTimeout, bool isISR)
{
	if(mtr->group_value == 0)
	{
		motor_stop_action(mtr);
		motor_stop_handle(mtr, isTimeout, isISR);
	}
	else
	{
		for(uint8_t i=0; i<g_motor_num; i++)
		{
			if(g_motors[i].group_value == mtr->group_value)
				motor_stop_action(&g_motors[i]);
		}
		for(uint8_t i=0; i<g_motor_num; i++)
		{
			if(g_motors[i].group_value == mtr->group_value)
				motor_stop_handle(&g_motors[i], isTimeout, isISR);
		}
	}
}

/**
 * @brief 单个电机复位
 */
static void motor_reset(MOTORS_CONTROL_S *mtr)
{
	if(mtr->group_value == 0)
	{
		if(mtr->location_value == M_CENTER)
		{
			log_d("motor%d location already at M_CENTER, don't need reset\r\n", mtr->index);
			return;
		}
		motor_stop(mtr, false, false);	/* 停止，可避免中点时同时操作多键引起的复位失效问题 */
		if(mtr->location_value == M_UP)
			motor_set_state(mtr, M_BACK, M_START_READY, true);
		else if((mtr->location_value == M_DOWN) || (mtr->location_value == M_UNKNOWN))	/* 位置为下或未知时先上移，上移无法到达中点时再下移 */
			motor_set_state(mtr, M_FORWARD, M_START_READY, true);
	}
	else
	{
		if(mtr->location_value == M_CENTER)
		{
			for(uint8_t i=0; i<g_motor_num; i++)
			{
				if(g_motors[i].group_value == mtr->group_value)
					log_d("motor%d location already at M_CENTER, don't need reset\r\n", g_motors[i].index);
			}
			return;
		}
		motor_stop(mtr, false, false);

		for(uint8_t i=0; i<g_motor_num; i++)
		{
			if(g_motors[i].group_value == mtr->group_value)
			{
				if(g_motors[i].location_value == M_UP)
					motor_set_state(&g_motors[i], M_BACK, M_START_READY, true);
				else if((g_motors[i].location_value == M_DOWN) || (g_motors[i].location_value == M_UNKNOWN))
					motor_set_state(&g_motors[i], M_FORWARD, M_START_READY, true);
			}
		}

	}
}

/**
 * @brief 电机操作（置位相关状态位） \n
 * 此接口对外（即按键或指令），电机为分组时则设置整个分组； \n
 * 此接口无阻塞，对同一个或一组电机的操作会执行最新的操作指令
 * @param  mtr         电机句柄
 * @param  m_state     动作状态
 */
static void motor_move(MOTORS_CONTROL_S *mtr, MOTOR_ACTION m_state)
{
	switch (m_state)
	{
	case M_STOP:
	{
		motor_stop(mtr, false, false);
			
	}break;
	
	case M_FORWARD:
	{
		if(mtr->group_value == 0)
			motor_set_state(mtr, M_FORWARD, M_START_READY, false);
		else
		{
			for(uint8_t i=0; i<g_motor_num; i++)
			{
				if(g_motors[i].group_value == mtr->group_value)
					motor_set_state(&g_motors[i], M_FORWARD, M_START_READY, false);
			}
		}
		
	}break;
	
	case M_BACK:
	{
		if(mtr->group_value == 0)
			motor_set_state(mtr, M_BACK, M_START_READY, false);
		else
		{
			for(uint8_t i=0; i<g_motor_num; i++)
			{
				if(g_motors[i].group_value == mtr->group_value)
					motor_set_state(&g_motors[i], M_BACK, M_START_READY, false);
			}
		}
	}break;
	
	case M_RESET:
	{
		motor_reset(mtr);
	}break;
	
	default:
		break;
	}
}

/**
 * @brief 单个电机执行动作（前进/后退）
 */
static bool motor_move_action(MOTORS_CONTROL_S *mtr)
{
	if(motor_get_move_state(mtr) == M_FORWARD)
	{
		xTimerReset(mtr->mtr_autoStop_timer, 0);
		GpioWrite( &mtr->Motor_ph1, 1 );
		GpioWrite( &mtr->Motor_ph2, 0 );
		xTimerReset(mtr->mtr_start_timer, 0);
	}
	else if(motor_get_move_state(mtr) == M_BACK)
	{
		xTimerReset(mtr->mtr_autoStop_timer, 0);
		GpioWrite( &mtr->Motor_ph1, 0 );
		GpioWrite( &mtr->Motor_ph2, 1 );
		xTimerReset(mtr->mtr_start_timer, 0);
	}
	else
		return false;
	
	motor_set_start_state(mtr, M_START_RUN);
	log_d("run=%d motor%d move_action %d\r\n", get_motor_startRun_count(), mtr->index, motor_get_move_state(mtr));
	return true;	
}

/**
 * @brief 移动电机组（分组尽量保证执行间隔小）
 * @param  group       组号
 * @param  m_state     （前进/后退）
 */
static void motor_move_action_group(uint8_t group, MOTOR_ACTION m_state)
{
	for(uint8_t i=0; i<g_motor_num; i++)
	{
		if(g_motors[i].group_value == group)
		{
			if(m_state == M_FORWARD)
			{
				GpioWrite( &g_motors[i].Motor_ph1, 1 );
				GpioWrite( &g_motors[i].Motor_ph2, 0 );
			}
			else if(m_state == M_BACK)
			{
				GpioWrite( &g_motors[i].Motor_ph1, 0 );
				GpioWrite( &g_motors[i].Motor_ph2, 1 );
			}
		}
	}

	for(uint8_t i=0; i<g_motor_num; i++)
	{
		if(g_motors[i].group_value == group)
		{
			xTimerReset(g_motors[i].mtr_autoStop_timer, 0);
			xTimerReset(g_motors[i].mtr_start_timer, 0);
			motor_set_start_state(&g_motors[i], M_START_RUN);
			log_d("run=%d motor%d move_action %d\r\n", get_motor_startRun_count(), g_motors[i].index, motor_get_move_state(&g_motors[i]));
		}
	}
}

/**
 * @brief 电机传感检测处理 	\n
 * @warning 运行于中断上下文
 */
static void sensor_interrupt_handle(MOTORS_CONTROL_S *mtr)
{
	log_d("motor%d interrupt_handle %d\r\n", mtr->index, GpioRead(&mtr->M_sensor_int));
	
	if((mtr->move_state == M_FORWARD) && (!mtr->move_reset))
	{
		if( (mtr->location_value != M_UP) && (GpioRead(&mtr->M_sensor_int) == 1) )		/* 上升沿离开中间 */
		{
			mtr->location_value = M_UP;
			ef_set_env_blob(mtr->location_key, &mtr->location_value, 1);
			log_d("motor%d location go to M_UP\r\n", mtr->index);
		}
		// else if(GpioRead(&mtr->M_sensor_int) == 0)
		// 	mtr->location_value = M_CENTER;
	}
	else if((mtr->move_state == M_BACK) && (!mtr->move_reset))
	{
		if( (mtr->location_value != M_DOWN) && (GpioRead(&mtr->M_sensor_int) == 1) )	/* 上升沿离开中间 */
		{
			mtr->location_value = M_DOWN;
			ef_set_env_blob(mtr->location_key, &mtr->location_value, 1);
			log_d("motor%d location go to M_DOWN\r\n", mtr->index);
		}
		// else if(GpioRead(&mtr->M_sensor_int) == 0)
		// 	mtr->location_value = M_CENTER;
	}
	else if(mtr->move_reset)
	{
		if(GpioRead(&mtr->M_sensor_int) == 0)
		{
			motor_stop(mtr, false, true);
		}
	}
}

/**
 * @brief 获取电机组启动中数量
 */
static uint8_t get_motor_startRun_count(void)
{
	uint8_t count = 0;
	for(uint8_t i=0; i<g_motor_num; i++)
	{
		if(motor_get_start_state(&g_motors[i]) == M_START_RUN)
			count++;
	}
	return count;
}

/**
 * @brief 获取电机组指定分组电机数量
 */
static uint8_t get_motor_group_count(uint8_t group)
{
	uint8_t count = 0;
	for(uint8_t i=0; i<g_motor_num; i++)
	{
		if(g_motors[i].group_value == group)
			count++;
	}
	return count;
}

/**
 * @brief 电机移动任务
 */
void motor_move_task( void *para )
{
	while(1)
	{
		for(uint8_t i=0; i<g_motor_num; i++)
		{
			if(get_motor_startRun_count() < MAX_MOTOR_START_NUM)	/* 启动中电机数量限制 */
			{
				if((motor_get_start_state(&g_motors[i]) == M_START_READY) && (g_motors[i].group_value > 0))			/* 处理分组 */
				{
					if( get_motor_group_count(g_motors[i].group_value) + get_motor_startRun_count() < MAX_MOTOR_START_NUM+1 )
						motor_move_action_group(g_motors[i].group_value, motor_get_move_state(&g_motors[i]));
				}
				else if((motor_get_start_state(&g_motors[i]) == M_START_READY) && (g_motors[i].group_value == 0))	/* 处理非分组 */
				{
					motor_move_action(&g_motors[i]);
				}
			}
		}

		vTaskDelay(5);
	}
}

/**
 * @brief 电机移动任务 \n
 * @warning 需在电机驱动及其实例初始化完成后调用
 */
static void motor_create_move_task(void)
{
	xTaskCreate( motor_move_task, "mm_task", 128, NULL, MOTOR_TASK_PRIO, NULL );
}


tMotorDriver MtrDriver;

/**
 * @brief 电机驱动初始化
 */
tMotorDriver* MotorDriverInit( MOTORS_CONTROL_S *mtr, uint8_t mtr_num )
{
	g_motors = mtr;
	g_motor_num = mtr_num;
	
	MtrDriver.Init = motor_init;
	
	MtrDriver.create_move_task = motor_create_move_task;
	
	MtrDriver.move = motor_move;
	
	MtrDriver.sensor_handle = sensor_interrupt_handle;
	
	
    return &MtrDriver;
}

#endif	/* BOARD_MOTOR_EN */

/*********** (C) COPYRIGHT 2020 FiberHome *****END OF FILE****/
